// ======================================================================
// Copyright (C) 2013 Hell-Prototypes. / www.hellprototypes.com
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2 of the License, or (at
// your option) any later version.
//
// This program is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Public License for more details.
// ======================================================================
#include "hell_arm.h"

bool pap_thread::set_picker_offset(int offset_x, int offset_y)
{
    m_picker_offset_x = offset_x;
    m_picker_offset_y = offset_y;

    return true;
}

iPoint pap_thread::pixel_coor_to_arm_arg(const Point &p_pixel)
{
    iPoint target_pos;

    double K = mm2mil(m_cam_width_length_mm)/(m_cam_image_width + 1e-10);
    int x_offset = (p_pixel.x - m_cam_image_width/2) * K / ONE_STEP_DISTANCE;
    int y_offset = (p_pixel.y - m_cam_image_height/2) * K / ONE_STEP_DISTANCE;

    target_pos.x = m_feeder_zone_center.x + x_offset - m_picker_offset_x;
    target_pos.y = m_feeder_zone_center.y - y_offset  - m_picker_offset_y;

    return target_pos;
}

bool pap_thread::place_coor_to_arm_arg(const DPoint *p_coor, iPoint &arm_arg)
{
    if((!m_pcb_mark_list.contains(0)) || (!m_pcb_mark_list.contains(1))) {
        emit disp_system_msg("pcb mark incorrect");
        return false;
    }
    pcb_mark_param mark0 = m_pcb_mark_list[0];
    pcb_mark_param mark1 = m_pcb_mark_list[1];

    double distance_pcb = distance_of_2_point(mark0.mark_coor.x, mark0.mark_coor.y, mark1.mark_coor.x, mark1.mark_coor.y);
    double distance_arm = distance_of_2_point(mark0.arm_pos.x, mark0.arm_pos.y, mark1.arm_pos.x, mark1.arm_pos.y);
    double K = distance_arm / (distance_pcb + 1e-10);

    int x_offset_0 = K * (p_coor->x - mark0.mark_coor.x);
    int y_offset_0 = K * (p_coor->y - mark0.mark_coor.y);
    int x_offset_1 = K * (p_coor->x - mark1.mark_coor.x);
    int y_offset_1 = K * (p_coor->y - mark1.mark_coor.y);

    int x_0 = mark0.arm_pos.x + x_offset_0;
    int y_0 = mark0.arm_pos.y + y_offset_0;
    int x_1 = mark1.arm_pos.x + x_offset_1;
    int y_1 = mark1.arm_pos.y + y_offset_1;

    arm_arg.x = (x_0 + x_1)/2;
    arm_arg.y = (y_0 + y_1)/2;

    return true;
}

double pap_thread::distance_of_2_point(double x0, double y0, double x1, double y1)
{
    double delta_x = x0 - x1;
    double delta_y = y0 - y1;

    return (double)sqrt(delta_x * delta_x + delta_y * delta_y);
}
